#include "AmoreDump.h"


AmoreDump::AmoreDump() {
	rollerBottom = new Relay(1, Relay::kBothDirections);
	rollerLift   = new Jaguar(7);
	shooterLift  = new Jaguar(5);
	limitLift    = new DigitalInput(1);

	liftTimer    = new Timer();
	liftTriggerTimer = new Timer();
	liftCurAction = 0;
	liftPrevAction = 0;
	
	collectorOn = true;
}


/*
 * This is the function that takes the lift up
 * 
 */

bool AmoreDump::liftUp() {
	collectorOn = false;
	
	if (liftTimer->Get() == 0) {
		// Timer has not been turned on yet
		liftTimer->Start();
	}
	
	// If the lift has been on less than 5 seconds
	// we want to keep it turned on for now
	if (liftTimer->Get() <= 5) {
		// Turn the lift on at half speed
		shooterLift->Set(0.5);
		std::cerr << "Lifting ..." << std::endl;
		return false;
	}
	// Otherwise we want to turn the lift on, but only at 1 tenth the full speed
	// so that it keeps the lift up high, but it won't pop a breaker.
	else {
		shooterLift->Set(0.1);
		std::cerr << "Holding ..." << std::endl;
		return true;
	}
	
	std::cerr << "Something is wrong!" << std::endl;
	return false;
}

bool AmoreDump::liftDown() {	
	if (limitLift->Get()) {
		// The lift is down at the bottom; set the motor speed to 0
		shooterLift->Set(0.0);
		std::cerr << "We are at the bottom" << std::endl;
		collectorOn = true;
		
		liftTimer->Stop();
		liftTimer->Reset();
		return true;
	} else {
		liftTimer->Stop();
		liftTimer->Reset();
		
		std::cerr << "Lowering ..." << std::endl;
		// Make it come back down at half speed, just like it went up
		shooterLift->Set(-0.5);
		return false;
	}
}

void AmoreDump::liftGo(Joystick * stick) {
	std::cerr << "\tCurrent liftCurAction: " << liftCurAction << std::endl;
	std::cerr << "\tliftPrevAction: " << liftPrevAction << std::endl;
	std::cerr << "\tlimitLift: " << limitLift->Get() << std::endl;
	
	if (liftCurAction == 0 || liftCurAction == 1) {
		// If we have a current action that is not yet complete, run that code.
		bool myReturn;
		
		if (liftCurAction == 1) {
			myReturn = liftUp();
		}
		
		if (liftCurAction == 0) {
			myReturn = liftDown();
		}
		
		if (myReturn == true) {
			// The return value was true, so the action is done
			liftCurAction = 3;
		}
		
		return;
	}

	if (stick->GetTrigger()) {
		if (liftPrevAction == 0) {
			// The previous action was to go down, so now we need to go back up
			liftCurAction = 1;
			liftPrevAction = 1;
			std::cerr << "We want to go up!" << std::endl;
		}
		else if (liftPrevAction == 1) {
			// Previous action was to go up, so now we need to go back down
			liftCurAction = 0;
			liftPrevAction = 0;
			std::cerr << "We want to go down!" << std::endl;
		}
		
		std::cerr << "\tTrigger was pulled!" << std::endl;
	}
	Wait(2);
}

void AmoreDump::rollerCollect() {
	std::cerr << "\tValue: collectorOn: " << collectorOn << std::endl;
	if (collectorOn == true) {
		std::cerr << "Relay is on" << std::endl;
		rollerBottom->Set(Relay::kForward);
		//rollerBottom->Set(Relay::kOn);
		//rollerLift->Set(0.5);
	}
	else {
		rollerBottom->Set(Relay::kReverse);
		std::cerr << "Relay is off" << std::endl;
		//rollerBottom->Set(Relay::kOff);
	}
}

void AmoreDump::dumpGo(Joystick * stick) {
	if (stick->GetRawButton(2)) {
		//rollerLift->Set(Relay::kReverse);
		std::cerr << "\tDumping ..." << std::endl;
	}
}

void AmoreDump::goRun(Joystick * stick) {
	//std::cerr << "liftGo" << std::endl;
	liftGo(stick);
	//std::cerr << "rollerCollect" << std::endl;
	rollerCollect();
	//std::cerr << "dumpGo" << std::endl;
	//dumpGo(stick);
}
